package org.parngon.maze;

//import java.awt.Point;

import java.util.ArrayList;
import java.util.LinkedList;
import java.util.HashMap;

import org.parngon.common.Coord;

public class AStar<T extends Coord>
{
  protected LinkedList<Tile<T>> open = null;
  //protected HashMap <Point,Tile>closed = null;
  protected HashMap<T, Tile<T>> closed = null;
  protected LinkedList<Tile<T>> solution = null;
  protected Tile<T> start = null;
  protected Tile<T> goal = null;

  protected Maze<T> map = null;

  public void setup(Tile<T> s, Tile<T> g, Maze<T> m)
  {
    start = s;
    goal = g;
    map = m;

    //closed = new HashMap<Point, Tile>();
    closed = new HashMap<T, Tile<T>>();

    open = new LinkedList<Tile<T>>();
    Tile<T> init = s;
    init.g(0);
    open.add(init);

    solution = new LinkedList<Tile<T>>();

    if ( start == goal || start.getLocation() == goal.getLocation() ) {
      // ...
      open.clear();
      solution.add(start);
    }
  }

  public boolean isCompleted()
  {
    return start != null && goal != null && open != null && open.size() == 0;
  }

  public LinkedList<Tile<T>> findPath(Tile<T> s, Tile<T> g, Maze<T> m)
  {
    setup(s, g, m);
    while ( open.size() > 0 ) {
      step();
//      System.out.println("Processing path, " + closed.size() + "/" + m.getSize().width * m.getSize().height + " nodes closed, " + open.size() + "/" + m.getSize().width * m.getSize().height + " nodes open");
    }
//    System.out.println("done");

    return getPath();
  }

  public void step()
  {
    Tile<T> best = null, parent = null;
    double bestF = Double.MAX_VALUE, bestG = 0;

    if ( open.size() > 0 ) {
      // search for the lowest cost node
      for ( Tile <T>aNode: open ) {
        double nodeF = aNode.f(goal);
        double nodeG = aNode.g();
        if ( best == null || bestF > nodeF || (bestF == nodeF && bestF - bestG > nodeF - nodeG) ) {
          best = aNode;
          bestF = best.f(goal);
          bestG = best.g();
        }
      }
//      System.out.println("Lowest = " + best + " (" + bestF + ")");
//      System.out.println("open = " + open);
//      System.out.println("closed = " + closed);
      open.remove(best);

      if ( best.getLocation().equals(goal.getLocation()) ) {
        closed.put(best.getLocation(), best);
        open.clear();
      } else {
        closed.put(best.getLocation(), best);
        parent = best;

        ArrayList<Tile<T>> neighbours = map.getNeighbours(parent);
        for ( Tile<T> aNeighbour: neighbours ) {
          //Point nLoc = aNeighbour.getLocation();
          T nLoc = aNeighbour.getLocation();
          if ( closed.containsKey(nLoc) ) {
//System.out.println("Neighbour " + aNeighbour + " is in closed");
            Tile<T> tmpNode = closed.get(nLoc);
            if ( tmpNode.g() > parent.g() + map.getTerrainCost(parent, tmpNode) ) {
              closed.remove(tmpNode);
              tmpNode.setParent(parent);
              tmpNode.g(parent.g() + map.getTerrainCost(parent, tmpNode));
              tmpNode.f(goal);
              closed.put(tmpNode.getLocation(), tmpNode);
            }
          } else {
            Tile<T> tmpNode = null;
            for ( Tile<T> aNode: open ) {
              if ( aNeighbour == aNode ) {
//System.out.println("Neighbour " + aNeighbour + " is in open");
                tmpNode = aNode;
              }
            }
            if ( tmpNode != null && tmpNode.g() > parent.g() + map.getTerrainCost(parent, tmpNode) ) {
              open.remove(tmpNode);
              tmpNode.setParent(parent);
              tmpNode.g(parent.g() + map.getTerrainCost(parent, tmpNode));
              tmpNode.f(goal);
              open.add(tmpNode);
            }
            if ( tmpNode == null ) {
              aNeighbour.g(parent.g() + map.getTerrainCost(parent, aNeighbour));
              aNeighbour.setParent(parent);
              aNeighbour.f(goal);
              open.add(aNeighbour);
            }
          }
        }
      }
    }

    if ( !closed.containsKey(goal.getLocation()) ) {
      solution.clear();
    } else {
      //Point aPoint = goal.getLocation();
      T aPoint = goal.getLocation();
      while ( aPoint != null ) {
        Tile aNode = closed.get(aPoint);
        solution.addFirst(aNode);
        if ( aNode.getParent() != start ) {
          aPoint = closed.get(aNode.getParent().getLocation()).getLocation();
        } else {
          aPoint = null;
        }
      }
      solution.addFirst(start);
    }
  }

  public LinkedList<Tile<T>> getPath()
  {
    return solution;
  }
}
